#include "cmsis_os.h"
#include "motor.h"
#include "Arm_Task.h"
#include "pid.h"
#include "main.h"
#include "remote_control.h"
#include "System_Task.h"
#include "ramp.h"
#include "referee_info.h"


Arm_Info_typedef Arm_Info;

PID_Info_TypeDef Arm_Pid[2][3][2];

static void Arm_Task_Init(void);
static void Arm_Solve(Arm_Info_typedef *Arm_Info);
static void Arm_OFF(Arm_Info_typedef *Arm_Info);
static void Arm_Standby(Arm_Info_typedef *Arm_Info);
static void Arm_Keep(Arm_Info_typedef *Arm_Info);
static void Data_solving(Arm_Info_typedef *Arm_Info);

float Arm_Pid_Param[][6] = {
    [J4310_Position] = {10, 0, 40, 0, 2000, 200},
    [J4310_Speed] = {40, 0.f, 5, 0, 4000, 6000},
    [J8009_Position] = {40, 0.f, 8, 0, 2000, 10000},
    [J8009_Speed] = {80, 0.2f, 0, 0, 2000, 40000},
};

void Arm_Task(void const *argument)
{
  /* USER CODE BEGIN Arm_Task */
  Arm_Task_Init();
  /* Infinite loop */
  for (;;)
  {
    Arm_Solve(&Arm_Info);
	Data_solving(&Arm_Info);
    switch (Arm_Info.Mode)
    {
    case 0:
    {
      Arm_OFF(&Arm_Info);
    }
    break;
    case 1:
    {
//      Arm_Standby(&Arm_Info);
//     Data_solving(&Arm_Info);
	    Arm_Keep(&Arm_Info);

    }
    break;
    case 2:
    {
      Arm_Keep(&Arm_Info);
    }
    break;
    }

    Arm_Info.Last_Mode = Arm_Info.Mode;

    osDelay(2);
  }
  /* USER CODE END Arm_Task */
}

void Arm_Task_Init(void)
{

  PID_Init(&Arm_Pid[0][0][0], PID_POSITION, Arm_Pid_Param[J4310_Position]);
  PID_Init(&Arm_Pid[0][0][1], PID_POSITION, Arm_Pid_Param[J4310_Speed]);
  PID_Init(&Arm_Pid[0][1][0], PID_POSITION, Arm_Pid_Param[J4310_Position]);
  PID_Init(&Arm_Pid[0][1][1], PID_POSITION, Arm_Pid_Param[J4310_Speed]);
  PID_Init(&Arm_Pid[0][2][0], PID_POSITION, Arm_Pid_Param[J4310_Position]);
  PID_Init(&Arm_Pid[0][2][1], PID_POSITION, Arm_Pid_Param[J4310_Speed]);
  PID_Init(&Arm_Pid[1][0][0], PID_POSITION, Arm_Pid_Param[J8009_Position]);
  PID_Init(&Arm_Pid[1][0][1], PID_POSITION, Arm_Pid_Param[J8009_Speed]);
  PID_Init(&Arm_Pid[1][1][0], PID_POSITION, Arm_Pid_Param[J8009_Position]);
  PID_Init(&Arm_Pid[1][1][1], PID_POSITION, Arm_Pid_Param[J8009_Speed]);
  PID_Init(&Arm_Pid[1][2][0], PID_POSITION, Arm_Pid_Param[J4310_Position]);
  PID_Init(&Arm_Pid[1][2][1], PID_POSITION, Arm_Pid_Param[J4310_Speed]);

  Arm_Info.Limit.S[0][0] = 0;
  Arm_Info.Limit.S[0][1] = 0;
  Arm_Info.Limit.S[1][0] = -134;
  Arm_Info.Limit.S[1][1] = 31;
  Arm_Info.Limit.S[2][0] = -287;
  Arm_Info.Limit.S[2][1] = 250;
  Arm_Info.Limit.B[0][0] = -110;
  Arm_Info.Limit.B[0][1] = -30 ;
  Arm_Info.Limit.B[1][0] = -115;
  Arm_Info.Limit.B[1][1] = -3;
  Arm_Info.Limit.B[2][0] = -237;
  Arm_Info.Limit.B[2][1] = 220;
  
  Arm_Info.Midangle.S[0]=27;
  Arm_Info.Midangle.S[1]= -57;
  Arm_Info.Midangle.S[2]= -17;
  Arm_Info.Midangle.B[0]= -19;
  Arm_Info.Midangle.B[1]= -3;
  Arm_Info.Midangle.B[2]= -18.5;
}

void Arm_Solve(Arm_Info_typedef *Arm_Info)
{

  switch (Arm_Info->Mode)
  {
  case 0:
  {
    for (int i = 0; i <= 2; i++)
    {
      Arm_Info->Target.S[i] = Arm_Motor[0][i].Data.angle;
      Arm_Info->Target.B[i] = Arm_Motor[1][i].Data.angle;
    }
  }
  break;
  case 1:
  {
//    for (int i = 0; i <= 2; i++)
//    {
//      Arm_Info->Target.S[i] = Arm_Motor[0][i].Data.angle;
//    }
    Arm_Info->Target.B[0] -= remote_ctrl.rc.ch[4] * 1e-4;
    Arm_Info->Target.B[1] -= remote_ctrl.rc.ch[1] * 1e-4;
  }
  break;
  case 2:
  {
//    Arm_Info->Target.S[0] += remote_ctrl.rc.ch[0] * 1e-6;
    Arm_Info->Target.S[1] -= remote_ctrl.rc.ch[4] * 2e-4;
    Arm_Info->Target.S[2] += remote_ctrl.rc.ch[0] * 2e-4;
    Arm_Info->Target.B[0] -= remote_ctrl.rc.ch[1] * 1e-4;
    Arm_Info->Target.B[1] -= remote_ctrl.rc.ch[3] * 1e-4;
    Arm_Info->Target.B[2] -= remote_ctrl.rc.ch[2] * 2.5e-4;
  }
  break;
  }
  for (int i = 0; i <= 2; i++)
  {

    VAL_LIMIT(Arm_Info->Target.S[i], Arm_Info->Limit.S[i][0], Arm_Info->Limit.S[i][1]);
    VAL_LIMIT(Arm_Info->Target.B[i], Arm_Info->Limit.B[i][0], Arm_Info->Limit.B[i][1]);
  }
}

void Arm_Keep(Arm_Info_typedef *Arm_Info)
{
  for (int i = 0; i <= 2; i++)
  {
    f_PID_Calculate(&Arm_Pid[0][i][0], Arm_Info->Target.S[i], Arm_Motor[0][i].Data.angle);
    Arm_Info->Torque.S[i] = f_PID_Calculate(&Arm_Pid[0][i][1], Arm_Pid[0][i][0].Output, Arm_Motor[0][i].Data.velocity);
    f_PID_Calculate(&Arm_Pid[1][i][0], Arm_Info->Target.B[i], Arm_Motor[1][i].Data.angle);
    Arm_Info->Torque.B[i] = f_PID_Calculate(&Arm_Pid[1][i][1], Arm_Pid[1][i][0].Output, Arm_Motor[1][i].Data.velocity);

    DM_MIT_Transform(Arm_Info->Send.S[i], &Arm_Motor[0][i], 0, 0, 0, 0, Arm_Info->Torque.S[i]);
    DM_MIT_Transform(Arm_Info->Send.B[i], &Arm_Motor[1][i], 0, 0, 0, 0, Arm_Info->Torque.B[i]);
  }

  if (Arm_Info->Last_Mode == OFF)
  {
    for (int i = 0; i <= 2; i++)
    {
      DM_Start(Arm_Info->Send.S[i]);
      DM_Start(Arm_Info->Send.B[i]);
    }
  }
  else if (Arm_Info->Last_Mode == STANDBY)
  {
    for (int i = 0; i <= 2; i++)
    {
//      DM_Lock(Arm_Info->Send.S[i]);
    }
  }
}

void Arm_Standby(Arm_Info_typedef *Arm_Info)
{
  Arm_Info->Target.B[2]=f_Ramp_Calc(Arm_Info->Target.B[2], 0, 0.05);

  for (int i = 0; i <= 2; i++)
  {

    f_PID_Calculate(&Arm_Pid[1][i][0], Arm_Info->Target.B[i], Arm_Motor[1][i].Data.angle);
    Arm_Info->Torque.B[i] = f_PID_Calculate(&Arm_Pid[1][i][1], Arm_Pid[1][i][0].Output, Arm_Motor[1][i].Data.velocity);

    DM_MIT_Transform(Arm_Info->Send.S[i], &Arm_Motor[0][i], 0, 0, 0, 0, 0);
    DM_MIT_Transform(Arm_Info->Send.B[i], &Arm_Motor[1][i], 0, 0, 0, 0, Arm_Info->Torque.B[i]);
  }

  if (Arm_Info->Last_Mode == KEEP)
  {
    for (int i = 0; i <= 2; i++)
    {
//      DM_Lock(Arm_Info->Send.S[i]);
    }
  }
  else if (Arm_Info->Last_Mode == OFF)
  {
    for (int i = 0; i <= 2; i++)
    {
	  DM_Start(Arm_Info->Send.S[i]);
      DM_Start(Arm_Info->Send.B[i]);
    }
  }
}

void Arm_OFF(Arm_Info_typedef *Arm_Info)
{
  for (int i = 0; i <= 2; i++)
  {

    DM_MIT_Transform(Arm_Info->Send.S[i], &Arm_Motor[0][i], 0, 0, 0, 0, 0);
    DM_MIT_Transform(Arm_Info->Send.B[i], &Arm_Motor[1][i], 0, 0, 0, 0, 0);
	  
	if(Arm_Motor[0][i].Data.error>11)
      DM_Reset(Arm_Info->Send.S[i]);
	
	if(Arm_Motor[1][i].Data.error>11)
      DM_Reset(Arm_Info->Send.B[i]);

  }

  if (Arm_Info->Last_Mode == KEEP)
  {
    for (int i = 0; i <= 2; i++)
    {
      DM_Lock(Arm_Info->Send.S[i]);
      DM_Lock(Arm_Info->Send.B[i]);
		
    }
  }
  else if (Arm_Info->Last_Mode == STANDBY)
  {
    for (int i = 0; i <= 2; i++)
    {
      DM_Lock(Arm_Info->Send.S[i]);
      DM_Lock(Arm_Info->Send.B[i]);

    }
  }
}

void Controller_Data_Update(uint8_t *data,uint16_t Size)
{
	  
	
	memcpy(&Arm_Info.Comm.Receive.Data[0],&Robot_Interactive.data[1],2);
	memcpy(&Arm_Info.Comm.Receive.Data[1],&Robot_Interactive.data[3],2);
	memcpy(&Arm_Info.Comm.Receive.Data[2],&Robot_Interactive.data[5],2);
	memcpy(&Arm_Info.Comm.Receive.Data[3],&Robot_Interactive.data[7],2);
	memcpy(&Arm_Info.Comm.Receive.Data[4],&Robot_Interactive.data[9],2);
	memcpy(&Arm_Info.Comm.Receive.Data[5],&Robot_Interactive.data[11],2);

;	
	
  }
//
void Data_solving(Arm_Info_typedef *Arm_Info)
{
  for(int i=0;i<=2;i++)
  {
  memcpy(&Arm_Info->Comm.Receive.Data[i],&Robot_Interactive.data[i*2+1],2);
  Arm_Info->Controller.S[i] = uint_to_float(Arm_Info->Comm.Receive.Data[i],-360,360,16);
  }
  for(int i=0;i<=2;i++)
  {
  
  memcpy(&Arm_Info->Comm.Receive.Data[i+3],&Robot_Interactive.data[i*2+7],2);
  Arm_Info->Controller.B[i] = uint_to_float(Arm_Info->Comm.Receive.Data[i+3],-360,360,16);
  }

  
}
